#include "Initialization.h"
#include "ceres/PBAFactors.h"
#include "DataStructure/LandMarkPLX.h"
#include <ceres/ceres.h>


namespace Estimator
{
void initialization::InitializeByManager(const Managing *pManager)
{
    mHashPoseTransBlock.reserve(pManager->mvpFrameContainer_.size());
    for (auto pframe : pManager->mvpFrameContainer_)
    {
        this->AddPoseBlock(pframe);
    }
}


void initialization::GenerateCostFunctionBeta(const Managing* pManaging)
{
    for(const auto landMark : pManaging->mlLandMarkContainer_)
    {
        const Frame *pMainFrame = landMark.GetMainFrame();
        const Frame *pAssoFrame = landMark.GetAssoFrame();

        if(pAssoFrame == nullptr)
            continue;

        auto mapObservations = landMark.GetObservations();

        for(auto iteObservations = mapObservations.cbegin(); iteObservations!= mapObservations.cend(); iteObservations++)
        {
            if(iteObservations->first == pMainFrame) continue;
            else if (iteObservations->first == pAssoFrame)
            {
                Eigen::Matrix3d information = Eigen::Matrix3d::Identity();
                // Add three Nodes Edge
                auto correspondence = iteObservations->first->mvLandMarkObservations_[iteObservations->second];
                // std::array<double,2> uv = correspondence.second;
                Eigen::Vector3d temp = mCalibration_.Kinv * Eigen::Vector3d(correspondence.second[0], correspondence.second[1], 1);
                temp.normalize();
                Eigen::Matrix3d Bij = landMark.GetInitializationParameter();
                Eigen::Matrix3d Roti = iteObservations->first->mRotationMatrix;
                double sintheta = landMark.GetSinParallax();

                ceres::CostFunction *cost_function = new CeresTransOnlyFactor3Nodes(temp, information,
                                                                                    Bij,
                                                                                    Roti,
                                                                                    sintheta/* , mCalibration_ */);
                mOptimizationProblem.AddResidualBlock(cost_function, NULL,
                                                      mHashPoseTransBlock[pMainFrame->GetFrameId()].parameters(),
                                                      mHashPoseTransBlock[pAssoFrame->GetFrameId()].parameters());
            }
            else
            {
                // Add four nodes edge.
                Eigen::Matrix3d information=Eigen::Matrix3d::Identity();
                // Add three Nodes Edge
                auto correspondence = iteObservations->first->mvLandMarkObservations_[iteObservations->second];

                Eigen::Vector3d temp = mCalibration_.Kinv* Eigen::Vector3d(correspondence.second[0], correspondence.second[1] ,1);
                temp.normalize();

                
                Eigen::Matrix3d Bij = landMark.GetInitializationParameter(); 
                Eigen::Matrix3d Roti = iteObservations->first->mRotationMatrix;
                double sintheta = landMark.GetSinParallax();

                ceres::CostFunction* cost_function = new CeresTransOnlyFactor4Nodes(temp, information, 
                                                                                    Bij,
                                                                                    Roti,
                                                                                    sintheta/* , mCalibration_ */);
                mOptimizationProblem.AddResidualBlock(cost_function, NULL, 
                                                        mHashPoseTransBlock[pMainFrame->GetFrameId()].parameters(), 
                                                        mHashPoseTransBlock[pAssoFrame->GetFrameId()].parameters(), 
                                                        mHashPoseTransBlock[iteObservations->first->GetFrameId()].parameters()
                                                      );

            }
        }
    }

    mOptimizationProblem.SetParameterBlockConstant(mHashPoseTransBlock.at(0).parameters());
    mOptimizationProblem.SetParameterBlockConstant(mHashPoseTransBlock.at(1).parameters());
}

// void initialization::GenerateCostFunction(const Managing* pManaging)
// {
      
//     // before you call this function, make sure that all poses and landmarks are added into optimizer;
//     for(const auto pFrame : pManaging->mvpFrameContainer_)
//     {
//         //AddPoseBlock(pFrame);
//         for (const auto observation : pFrame->mvLandMarkObservations_)
//         {
//             size_t landMarkid = observation.first;
//             auto landmarkPos = std::find_if(pManaging->mlLandMarkContainer_.cbegin(), pManaging->mlLandMarkContainer_.cend(),
//                                             [&landMarkid](const LandMarkPLX &landmark) { return (landmark.GetId() == landMarkid);});

//             if (landmarkPos == pManaging->mlLandMarkContainer_.cend()){
//                 std::cerr << "GenerateCostFunction : no such landmark id:  " << landMarkid << std::endl;
//                 continue;
//             }
            

//             // Get the Main and Associate Anchor of current landmark:
//             const Frame *pMainFrame = landmarkPos->GetMainFrame();
//             const Frame *pAssoFrame = landmarkPos->GetAssoFrame();
            
//             if (pFrame->GetFrameId() == pMainFrame->GetFrameId())
//                 continue;
//             else if (pFrame->GetFrameId() == pAssoFrame->GetFrameId())
//             {
//                 // nEdge_++;
//                 Eigen::Matrix3d information = Eigen::Matrix3d::Identity();
//                 // Add three Nodes Edge

//                 Eigen::Vector3d temp = mCalibration_.Kinv* Eigen::Vector3d(observation.second[0], observation.second[1] ,1);
//                 temp.normalize();
//                 Eigen::Matrix3d Bij = landmarkPos->GetInitializationParameter();
//                 Eigen::Matrix3d Roti = pFrame->mRotationMatrix;
//                 double sintheta = landmarkPos->GetSinParallax();

//                 ceres::CostFunction* cost_function = new CeresTransOnlyFactor3Nodes(temp, information, 
//                                                                                     Bij,
//                                                                                     Roti,
//                                                                                     sintheta,mCalibration_);
//                 mOptimizationProblem.AddResidualBlock(cost_function, NULL, 
//                                                         mHashPoseTransBlock[pMainFrame->GetFrameId()].parameters(), 
//                                                         mHashPoseTransBlock[pAssoFrame->GetFrameId()].parameters()
//                                                         );
                

//             }
//             else
//             {
//                //  nEdge_++;
//                 // Add four nodes edge.
//                 Eigen::Matrix3d information=Eigen::Matrix3d::Identity();
//                 // Add three Nodes Edge
//                 Eigen::Vector3d temp = mCalibration_.Kinv* Eigen::Vector3d(observation.second[0], observation.second[1] ,1);
//                 temp.normalize();

//                 Eigen::Matrix3d Bij = landmarkPos->GetInitializationParameter(); 
//                 Eigen::Matrix3d Roti = pFrame->mRotationMatrix;

//                 double sintheta = landmarkPos->GetSinParallax();

//                 ceres::CostFunction* cost_function = new CeresTransOnlyFactor4Nodes(temp, information, 
//                                                                                     Bij,
//                                                                                     Roti,
//                                                                                     sintheta, mCalibration_);
//                 mOptimizationProblem.AddResidualBlock(cost_function, NULL, 
//                                                         mHashPoseTransBlock[pMainFrame->GetFrameId()].parameters(), 
//                                                         mHashPoseTransBlock[pAssoFrame->GetFrameId()].parameters(), 
//                                                         mHashPoseTransBlock[pFrame->GetFrameId()].parameters()
//                                                       );

//             }
//         }
//     }

//     mOptimizationProblem.SetParameterBlockConstant(mHashPoseTransBlock.at(0).parameters());
//     mOptimizationProblem.SetParameterBlockConstant(mHashPoseTransBlock.at(1).parameters());

// }

void initialization::AddPoseBlock(const Frame* pframe)
{
    // Eigen::Matrix<double, 3, 4, RowMajor> frameTransMat;
    // frameTransMat.topLeftCorner<3,3>() = pframe->mRotationMatrix;
    Eigen::Vector3d frametrans = pframe->mTranslationVec;

    // save it into Pose Vector
    // mvPoseTransBlock.emplace_back(pframe->GetFrameId(), frametrans);
    mHashPoseTransBlock.emplace(std::piecewise_construct, std::forward_as_tuple(pframe->GetFrameId()),
                                                        std::forward_as_tuple(pframe->GetFrameId(), frametrans));
    mOptimizationProblem.AddParameterBlock(mHashPoseTransBlock[pframe->GetFrameId()].parameters(), PoseTransBlock::Dimension);

}
}